#! /usr/bin/env python
"""
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)


    消息订阅方:
        订阅话题并打印接收到的消息

    实现流程:
        1.导包 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 订阅者 对象
        4.处理订阅的消息(回调函数)
        5.设置循环调用回调函数



"""
#1.导包 
import rospy
from std_msgs.msg import String
# import time
from geometry_msgs.msg import Twist
from threading import Thread
from log.pylog import log
LOGS_INFO = log.info
'''
    获取雷达测距参数方法 Version 2.0
        在速度上优于get_laser2.py 三倍
        通过对比 10 次读取耗时
        single_laser.py     get_laser2.py
        0.9905 s              2.8756 s 
''' 



class laser_dis:
    def __init__(self) -> None:
        self.dis = 0
    def set_dis(self,distance):
        self.dis = distance
    def get_dis(self):
        return self.dis

single_laser = laser_dis()


def doMsg(msg):
    global single_laser
    single_laser.set_dis(float(msg.data))
    # rospy.loginfo("I heard:%s",msg.data)


def forwrd_p(cmd_vel,move_cmd):
    rate = 50
    r = rospy.Rate(rate)
    while True:
        if move_cmd.linear.x >= -0.01:
            break
        cmd_vel.publish(move_cmd)
        r.sleep()

def adopt_forward(speed=-0.30,target_dis=0.2):
    global single_laser
    cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
    move_cmd = Twist()
    # 初始设置速度
    # 设置目标距离
    move_cmd.linear.x = speed
    # 获取真实距离
    laser_dis = single_laser.get_dis()
    print(laser_dis)
    init_minus_val = laser_dis - target_dis
    forward_thread = Thread(target=forwrd_p,args=[cmd_vel,move_cmd])
    forward_thread.start()
    while laser_dis > target_dis :
        rospy.loginfo('distance of the obstacle : %f', laser_dis)
        laser_dis = single_laser.get_dis()
        # 速度循环调节
        if speed < -0.05:
            speed = -0.05
            minus_val = laser_dis - target_dis
            speed *= minus_val/init_minus_val 
            LOGS_INFO("distance of the obstacle : {}".format(laser_dis))

        move_cmd.linear.x = speed 
        # rospy.loginfo("speed:{} ".format(move_cmd.linear.x))
        if speed > -0.02:
            break
        rospy.sleep(0.05)
    move_cmd.linear.x = 0
    cmd_vel.publish(Twist())
    if forward_thread.isAlive():
        forward_thread.join()

def subscribe_main():
    # rospy.init_node("subscribe_single_laser")
    #3.实例化 订阅者 对象
    sub = rospy.Subscriber("chatter",String,doMsg,queue_size=10)
    #4.处理订阅的消息(回调函数)
    #5.设置循环调用回调函数
    rospy.spin()


if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("subscribe_single_laser")
    #3.实例化 订阅者 对象
    sub = rospy.Subscriber("chatter",String,doMsg,queue_size=2)
    # 4.处理订阅的消息(回调函数)
    #5.设置循环调用回调函数
    rospy.spin()


